function [uc_opt_, u_opt_] = TubeNMPC_Controller(ref, g, g_sim, u_r) %#codegen
% This function
% Name          :TubeNMPC_Controller
% Descriptions  :使用TubeNMPC方法控制，得到速度的控制输出
% Argument(s)   :g:当前位姿xyθ; ref:参考位姿序列xyθ; u_r:参考输入
% Return(s)     :u_(1):得到的速度v;u_(2):得到的角速度Ω
% Author        :LHP,on March 3rd,2024

%% Problem formulation
% 定义NMPC的参数
N = 10;                                     % 预测时域 N * dt
T = 0.6;                                    % 仿真时长 T * dt
Q = diag([1.0, 1.0, 1.0]);                  % 状态权重矩阵 [5.0, 5.0, 5.0]
R = diag([1.0, 1.0]);                       % 输入权重矩阵
P = Q;                                      % 终端状态权重矩阵

%% Disturbance
Bw = [1,0,0;0,1,0;0,0,3];                 % 扰动输入矩阵

wmax_x = 0.0006;                             % 状态x扰动的最大值0.01m/s 0.006
wmax_y = 0.0006;                             % 状态y扰动的最大值0.01m/s
wmax_theta = 0.0006;                         % 状态theta扰动的最大值0.03rad/s

wmax = [wmax_x; wmax_y; wmax_theta];
wmax_norm = norm(wmax);

%% Initial settings
uM = [0.5; 0.5];                          % 控制输入u的约束条件v_max,w_max
xM = [0.07; 0.07; 0.15];                     % 状态误差deltax的约束 xM = [0.02; 0.02; 0.02];

%% Caculate the robust invariant set (RIS)=鲁棒控制不变集

% 离散线性化差速机器人误差模型得到A和B
[A,B] = linear_errmodel(u_r(1), u_r(2), T); %% (ref_v(k), ref_theta(k), T)

[P1, K, lambda0, mu] = NMPC_get_ris_new (A, B, Bw); 

% 图: 最小鲁棒正不变集的形状
%draw_ellip(P1, mu * wmax_norm^2/lambda0, 'b') % 绘制椭圆
%hold on

% 在鲁棒控制不变集范围内的受最大程度干扰情况下控制输入u的优化计算问题
yalmip('clear');
x = sdpvar(3,1);                            % 定义优化变量
% 约束条件:确保x属于robust invariant set的前提下进行优化
const = x' * P1 * x <= (mu * wmax_norm^2 / lambda0); 
% 目标函数
obj1 = K * x; 
% 最小化目标函数obj1，并且满足约束条件const
ops = sdpsettings('verbose', 0);
optimize(const, obj1, ops);                 % 优化
u_0 = value(obj1);                          % 获取优化结果:鲁棒不变集得到的控制量约束

%% Get the terminal region Xf and terminal penalty for nominal MPC=终端最大鲁棒正不变集

% alphaM被用作名义MPC中终端区域和终端权重的上界
alphaM = 5;                                 % 越小，tube越小 alphaM = 50000

[P_nom, K0_nom, alpha_nom] = NMPC_get_max_terminal_tube_new(Q, R, uM + u_0, xM, alphaM, A, B);
alpha_nom;
P_nom;

% 图: 最大终端不变集范围
%draw_ellip(P_nom, alpha_nom, 'g')
%legend({'mRIS','MPI'},'Orientation','horizontal');

%% NMPC Optimization problem using YALMIP

% 更新状态
x0 = g_sim;                                 % g_sim
tilde_x = g - g_sim;                        % 误差反馈 g - g_sim

% 定义决策变量x和u为优化变量
yalmip('clear');
X = sdpvar(3, N);                           % 状态变量
U = sdpvar(2, N-1);                         % 输入变量

% 约束(硬约束:状态或输入的边界条件)
const = [];
for i = 1:N-1
    const = [const, X(:, i + 1) == robot_model(X(:, i), U(:, i), T)];   % 添加状态转移约束
end
const = [const, X(:, 1) == x0];             % 添加初始状态约束

% tube
const = [const, (X(:, N) - ref(:,N))' * P_nom * (X(:, N) - ref(:,N)) <= alpha_nom]; % x ∈ Xf:terminal set P_nom
const = [const, U(1, :) >= (-uM(1) - u_0(1)), U(1, :) <= (uM(1) + u_0(1))]; 
const = [const, U(2, :) >= (-uM(2) - u_0(2)), U(2, :) <= (uM(2) + u_0(2))]; 

% 目标函数(性能指标+惩罚项软约束)
obj = 0;
for i=2:N-1
    e_x = X(1, i) - ref(1,i);
    e_y = X(2, i) - ref(2,i);
    e_theta = X(3, i) - ref(3,i);
    e = [e_x; e_y; e_theta];
    u = U(:, i);
    %deltau = abs(U(:, i) - U(:, i-1));
    obj = obj + e' * Q * e + u' * R * u;% + deltau' * 0.5*R * deltau;
end

% tube
obj = obj + (X(:,end) - ref(:,N))' * P_nom * (X(:,end) - ref(:,N));% x ∈ Xf:terminal set P_nom

% 求解优化问题
ops = sdpsettings('solver', 'fmincon', 'fmincon.maxiter', 100, 'fmincon.tolx', 1e-6, 'verbose', 0); %求解器/迭代次数/容许误差/显示级别
optimize(const, obj, ops);

% 提取最优控制输入
u_opt = value(U(:, 1));

% tube
uc_opt = u_opt + K * tilde_x;

%% TubeNMPC控制量输出
uc_opt_ = uc_opt;
u_opt_ = u_opt;

end